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DESCRIPTION 
CONTROL APPARATUS OF ROBOT 

Technical Field 
[0001] 

The present invention relates to a control apparatus of 
a robot, which controls the robot constituted by a plurality 
of shafts interfering with each other in such a manner that 
even when interference force is exerted, the respective shafts 
can be operated in correspondence with instructions. 

Related Art 
[0002] 

A conventional robot control apparatus is provided with 
a model controller . The model controller contains a pseudomodel , 
a control unit for controlling the pseudomodel, and a correction 
amount calculating unit for calculating model compensating 
torque in order to cancel interference torque . The conventional 
robot control apparatus performs an interference free operation 
by outputting a model feedforward instruction to which the model 
compensating torque is added (refer to, for example, patent 
publication 1 ) . 

Fig. 5 is a control block diagram for indicating an 
arrangement of a conventional robot control apparatus according 
to an embodiment mode, which is disclosed in the patent 
publication 1. Fig. 6 is a control block for representing an 
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arrangement of a model controller of Fig . 5. In Fig. 6, symbols 
"54U" and "54L" indicate correction amount calculating units, 
and symbols "60U" and "60L" represent output signals to feedback 
control systems. In Fig. 5, a basic construction of robot 
control systems of two shafts which are approximated to 2 inertia 
systems (spring + arm such as motor + speed reducer etc.) with 
respect to each of these shafts. In Fig. 5 and Fig. 6, it is 
so assumed that two shafts are assumed as XX L shaf t" and "U shaft", 
as to an instruction and an amount related to the L shaft, symbol 
"_L" is added to a suffix thereof, whereas as to an instruction 
and an amount related to the U shaft, symbol "_U" is added to 
a suffix thereof. 
[0003] 

This robot control apparatus performs a control operation 
as to the L shaft and the U shaft based upon motor position 
instructions "X ref _ L " and "X re f_u" with respect to each of the 
shafts. The robot control apparatus has been provided with 
feedback control systems (feedback controllers) 53L, 53U of 
the two shafts; and a model controller 51 corresponding to a 
pseudomodel with respect to a control subject (motors, speed 
reducers, and robot arms) . While themodel controller 51 inputs 
the motor position instructions "X re f_ L " and "X re f_u", this model 
controller 51 calculates feedforward instructions "U F f_l" and 
"U F f_u" of the respect shafts by considering dynamics of a robot 
so as to execute a feedforward compensating operation with 
respect to an acceleration term of the motor of each of the 
shafts. Further, the model controller 51 calculates motor 
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positions "0Mm_L" and "6 M m_u" of each of the shafts in a standard 
model; calculates motor speeds thereof: 
[Formula 1] 



and calculates torsion angles thereof "6 M s_l" and "0 M s_u"; and 
further calculates torsion angular velocities thereof: 
[Formula 2] 



and then outputs the calculated results. 
[0004] 

It should be understood that a prefix "model" is added 
to each of elements corresponding to outputs of the model 
controller 51. 

On the other hand, in the feedback control systems 
(feedback controllers) 53L and 53U, positional gains of the 
respective shafts are XX K P _ L " and XX K PL "; speed gains thereof are 
"Ku l" and "K v _u"; torque constants thereof are xx K t _ L " and xx K t _ v "; 
and final acceleration instructions applied to the motors are 
"U re f_L" and "U re f_u", respectively. Further, inertia moment of 
the motors of the respective shafts is x> J m _ L " and xx J m _u"; and 
current instructions thereof are x> I re f_L" and "I re f_u." Actual 
positions of the motors are xx 6 m _ L " and xx 6 m _u"; actual speeds of 
the motors are: 
[Formula 3] 




9 *sL . 9 UsJ 



and also, actual acceleration of the motors are: 
[Formula 4] 

In addition, speed reducing ratios in the speed reducers 
are expressed as "N_ L " and >x N_u"; spring constants of the speed 
reducers are expressed as "K C _ L " and "K cU "; inertia moment of 
the arms are expressed as "J l _l" and xx J L _u" ; actual positions 
of the arms are expressed as "9l_l" and "9l_u"; and acceleration 
(load acceleration) of the arms are represented by the following 
formula 5: 
[Formula 5] 

Then, in order to represent interference between both 
the shafts, a value of M L u/M 0 2 which determined by the mass of 
the robot arms and the angle between both the shafts is multiplied 
by load torque of the L shaft, so that the multiplied value 
is exerted to the load acceleration of the U shaft. Similarly, 
a value of M UL /M 0 2 is multiplied by the load torque of the U 
shaft, so that the multiplied value is exerted to the load 
acceleration of the L shaft. 
[0005] 

In this robot control system, status monitoring devices 
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52L and 52U have been provided with respect to the L shaft and 
the U shaft. The status monitoring devices 52L and 52U input 
the final acceleration instructions U re f_L and U re f_u applied to 
the motors, and the actual positions 6 m _ L an <3 9mjj of the motors 
so as to predict actual torsion angles 0 S _ L and 6 s _u, and angular 
velocities thereof : 
[Formula 6] 



Then, in this feedback controller, the positional gains 
Kp_ L and k p _u are multiplied with respect to deviation between 
the model motor positions 6 M m_L and 6 Mm _u outputted from the model 
controller 51, and the actual motor positi ons 0 m l and Gm u/ and 
then, the multiplied position gains are employed as speed 
instructions of the respective shafts. 

Further, the below-mentioned deviation is added to each 
of the speed instructions obtained in the above-explainedmanner , 
and then, the velocity gains K v _l and K v _u are multiplied with 
aspect to the added speed instructions, which are used as 
acceleration instructions. The above-described deviation is 
defined between the model motor speeds: 
[Formula 7] 





and the actual motor speeds: 



[Formula 8] 



[0006] 

With respect to these acceleration instructions, such 
values obtained by multiplying the following deviation by 
feedback gains Ki_ L and Ki_u are added, and further such values 
obtained by multiplying the below-mentioned deviation by 
feedback gains K 2 _ L andK 2 _u are added. Then, the resultant values 
are defined as the final motor acceleration instructions U re f_L 
and U re f_u with respect to the respective shafts. In this case, 
the first-mentioned deviation is defined by (a) the feedforward 
instructions U F f_l and U FF _u derived from the model controller 
51, (b) the model torsion angles 9 Ms _l and 6 Ms _u, and the torsion 
angles 0 S _ L and 0 s _u, outputted from the status monitoring devices 
52L and 52U. The last-mentioned deviation is defined by (c) 
the model torsion angular velocities: 
[Formula 9] 

and the torsion angular velocities outputted from the status 
monitoring devices 52L and 52U: 
[Formula 10] 
• • • . 

In this robot control system, the motors of the respective 
shafts are driven based upon the final motor acceleration 
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instructions U re f_L and U re f_u obtained in the above-explained 
manner, and thus, the arms of the respective shafts are driven 
via the speed reducers, the speed reducing ratios of which are 
N_ L and N_u respectively. In this case, as previously explained, 
interference force between the shafts is exerted to the load 
acceleration of such of the shafts. 
[0007] 

Next, a description is made of an arrangement of the model 
controller 51. Fig. 6 represents the detailed arrangement of 
the model controller 51 which is arranged by feedback control 
systems, motors, speed reducers, and pseudomodels indicative 
of robot arms. Positional gains and speed gains of the 
respective shafts in a pseudomodel are model positional gains 
"K pMJj " and "K pM _u"; and model speed gains "K vM _l" and "K v m_u"/ 
respectively. Similarly, model motor inertia moment "J^l" and 
"JmM_u"/ model arm inertia moment " Jlm_l" and "J L m_u"/ model speed 
reducing ratios "N M _ L " and "N M _u", and model speed reducer spring 
constants xx K cM L " and "K cMJ j" have been determined as model 
parameters . 

The model controller 51 inputs the motor position 
instructions X re f_L and X re f_u of the respective shafts, and 
multiplies such a deviation between the motor position 
instructions X re f_ L and X ref u , and the model motor positions 0mM_L 
and 0mM_u by the model position gains K pM _l and K pM _u so as to define 
the multiplied values as model speed instructions of the 
respective shafts. Then, the model controller 51 multiplies 
such values by the model speed gains K vM L and K vM _u, and defines 
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the multiplied values as model acceleration instructions of 
the respective shafts. The above-explained values are 
calculated by subtracting model motor speeds of the respective 
shafts from these model speed instructions, while these model 
motor speeds are expressed by the following formula: 
[Formula 11 ] 



The model controller 51 subtracts the above-described 
model acceleration instructions from both a value which is 
obtained by multiplying the model torsion angles 0 M s_l and 9 M s_u 
derived from the model speed reducer by the model feedback gains 
K iM _L and K iM _u/ and another value which is obtained by multiplying 
model torsion angular velocities by the model feedback gains 
K2m_l and K 2 m_u- The above-explained model torsion angular 
velocities are obtained by differentiating the model torsion 
angles 6 M s_L and 0 M s_u/ and are expressed by the following formula : 
[Formula 12] 



The model controller 51 defines the subtracted values 
as model acceleration instructions "U M ref_i/' and "U Mr ef_u", which 
are inputted to the model motors. 

As a consequence, the model acceleration instructions 
U M ref_L andU Mr ef_umaybe expressed by the below-mentioned formulae : 
[Formula 13] 




[0008] 
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■ 

" K 1M_L * &MsL ~ KZML ' 6 Ms J, CD 
" K 1M JJ " ^Ms_U ~ K 2MJJ " ^Ms.U ® 

However, in the two shafts robot predicted in this example, 
due to interference, the load torque of the L shaft is multiplied 
by such a value of "M L um/M 0 m 2 '', and then, the multiplied load 
torque is exerted to the load acceleration of the U shaft. This 
value is determined by the mass of the robot arms and the angle 
between both the L shaft and the U shafts. Similarly, the load 
torque of the U shaft is multipliedby another value of "Mulm/M 0 m 2 ", 
and then, the multiplied load torque is exerted to the load 
acceleration of the L shaft. 
[0009] 

Assuming now that such an interference is grasped as 
disturbance which is exerted to an arm side, as to the L shaft, 
if model correction torque "T com p_ L " shown in the below-mentioned 
formula (14) is added to the model motor acceleration instruct ion 
"U M ref_L"/ then vibrations of the arm caused by the interference 
can be reduced: 
[Formula 14] 
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rp K pM.L ,K vM.L' N M.L' D i5.UL K vM_l/ N M_L ,D is_UL K lM_L' D iS-UL 

CWnp - L ^cM.L K cM_L KcML 

K 2MJ/ D is_UL D is.UL N M.L' D is_XJL ^ 
KcM.L JmM.L' N M_L ^cM_L 

In this formula, symbol w D is _ui." indicates such an 
interference torque which is exerted from the U shaft to the 
L shaft, and this interference torque may be expressed by the 
following formula: 
[Formula 15] 



%.UL~JuW.L*K cM U - njr 2 -^Msjj (4) 



U ULM 

As a consequence, while a final model motor acceleration 
instruction "U FF _ L " is .calculated as 

Uff_L = UMref_L + T CO mp_L (5) , 

a feedforward compensating operation is carriedout with respect 
to an acceleration term of a motor. This model controller 51 
has been provided with the correction amount calculating unit 
54L which inputs the model torsion angle "9 M s_u" and calculates 
the model correction torque "T CO m P _L" based upon the 
above-explained formulae (3) and (4) . Thus, the model 
correction torque "T comp _ L " derived from the correction amount 
calculating unit 54L is added to the model motor acceleration 
instruction U Mr ef_L. 
[0010] 

Similarly, as to the U shaft, model correction torque 
comp L " is defined as the following formula: 
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[Formula 16] 



T, 



C ° mP " U K cMJJ KcM.U K cM_U 

K 2M.U* D is_LU D is.LU N M_U* D is_LU 



K cM_U JmM_U" N M.U K cM_U 



(6) 



In this formula, symbol "D is LU " shows interference torque 
which is exerted from the L shaft to the U shaft, and is expressed 
by the following formula: 
[Formula 17] 

M LUM 

Dis.LU = JlM u'KcM L~ 2"°^Ms_L (7) 

" Km 

The model controller 51 has been provided with the 
correction amount calculating unit 54U which inputs the model 
torsion angle "8 M s_l" f and calculates the model correction torque 
"T C omp_u" based upon the formulae (6) and (7) . As a result, a 
final model motor acceleration instruction "U F f_u" becomes: 
U ff_u = U Mref U + T conj p_u ( 8 ) 

[0011] 

As previously described, in the conventional technique 
disclosed in the patent publication 1, after the interference 
torque is corrected by the model controller 51 in the 
above-explained manner, the respective status mounts which 
constitute the standard status amounts are entered as the 
instructions to the feedback control systems. As a result, 
the respective robot arms are not adversely influenced by the 
interference caused by another shaft. 
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As previously described, in the conventional robot 
control apparatus , the interference force received from another 
shaft is corrected by the model controller, and then, based 
upon the corrected interference, the respective status amounts 
which become the standard status amounts are inputted to the 
feedback controller. As a result, the noise component is not 
superimposed on the corrected torque, and the robot arm can 
improve the locus precision of the tool tip portions without 
receiving of vibrations caused by the interference. 
[0012] 

Patent Publication 1: JP-A-1 0-329063 (Figures 1 and 2, 
pages 2 to 5) . 

Disclosure of the Invention 

Problem that the Invention is to Solve 

[0013] 

However, in the conventional robot control apparatus, 
in order to realize this controller, the model controller 51 
must perform the calculation of the pseudomodel such as the 
calculation of the torsion angles for every control period other 
than the calculations of the feedback control unit. Therefore, 
the conventional robot control apparatus owns such a problem 
that a total amount of these calculations with respect to each 
of the shafts becomes very large. 

Further, the model controller 51 must execute the 
calculation of the model-purpose feedback control operations 
so as to control the pseudomodels for every control period other 
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than the calculation of the feedback control unit. As a result, 
there is another problem that a total amount of these 
calculations with respect to each of the shafts is further 
increased. 

Further, since the interference free operation is carried 
out only by correcting the torque, the values as to the model 
position gain x> K pM " and the model speed gain "K v m" are required 
in order to calculate the model correction torque T C om P - As a 
result, there is another problem that the calculation becomes 
more complex. 

As a result, the calculation amounts for each of these 
shafts become huge, and the control operation time is prolonged, 
so that there is such a problem that a high speed CPU is required 
so as to realize this robot control apparatus. 

The present invention has been made to solve these problems , 
and therefore, has an object to provide such a control apparatus 
of a robot which is constituted by a plurality of shafts 
interfering with each other. In the robot control apparatus 
which is equipped with a position control unit and a speed control 
unit in order to operate each of the shafts in correspondence 
with an instruction for each of the plural shafts, while each 
of the shafts is constituted by an arm coupled to a motor via 
a spring element (spring constant) such as a speed reducer, 
and a motor position detector for detecting a position of the 
motor, such a robot control apparatus can be provided: The motor 
can be operated in such a manner that the own shaft is operated 
in correspondence with the instruction also in such a case that 
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interference force exerted from another shaft is present; the 
robot arm can be operated without receiving adverse influences 
by interference; and a calculation amount is largely reduced, 
so that the load given to the CPU can be reduced. 

Further, another object of the present invention is to 
provide a robot control apparatus operable as follows: That 
is, also in such a case that interference force is exerted from 
the plural shafts to the own shaft, the process, operation for 
calculating a total value of the interference force exerted 
from the respective shafts to the own shaft is merely carried 
out, while a relationship among the plural shafts need not be 
considered but also a complex calculation is not carried out. 
Subsequently, the interference free operations among the plural 
shafts can be simply carried out by merely performing a process 
operation similar to the inference free operation between the 
two shafts. 

Further, in the case that interference between shafts 
which have not been considered is added, while a program is 
not substantially amended, the interference free operations 
for all of the shafts can be simply realized by merely adding 
such an operation for calculating a total value of the 
interference force exerted from the respective shafts. 

Means for Solving the Invention 
[0014] 

To solve the above-explained problems, according to a 
first aspect of the present invention, there is provided a 
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control apparatus of a robot including: 

a plurality of shafts interfering with each other; 

each of the shafts including a motor, an arm coupled to 
the motor via a spring element such as a speed reducer and a 
motor position detector for detecting a position of the motor; 

a position control unit and a speed control unit in order 
to operate each of the shafts in correspondence with an 
instruction for each of the plural shafts; 

an interference force calculating unit for calculating 
interference force which is exerted to another shaft from an 
instruction of the own shaft; 

a non-interference torque signal forming unit for forming 
a motor torque instruction signal by which the own shaft is 
operated in correspondence with the instruction also in such 
a case that interference force exerted from another shaft is 
present based upon the calculation value of the interference 
force exerted from another shaft, and the instruction of the 
own shaft; and 

a non-interference position signal producing unit for 
producing a motor position signal by which the own shaft is 
operated in correspondence with the instruction also in such 
a case that interference force executed from another shaft is 
present based upon the calculation value of the interference 
force exerted from another shaft, and the instruction of the 
own shaft. 

In accordance with the first aspect, while the robot 
control apparatus contains the interference force calculating 
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unit, the non-interference torque signal producing unit, and 
the non-interference position signal producing unit, this robot 
control apparatus does not calculate a pseudomodel but also 
not calculate a control for the pseudomodel, and furthermore, 
produces not only the motor torque signal but also the motor 
position signal by considering the interference. As a result, 
the calculation amounts can be largely lowered. 
[0015] 

According to a second aspect of the present invention, 
there is provided the robot apparatus as claimed in the first 
aspect, wherein 

with respect to interference in the case that the shafts 
are two shafts, 

interference force in the interference force calculating 
unit is calculated based upon the below-mentioned formulae: 

d isl =B * x refl * s 2 

d is2 = A * x ref2 * s 2 

a non-interference torque signal in the non-interference 
torque signal producing unit is calculated based upon the 
below-mentioned formulae : 

t re f_ffl= (J.1* Ju/Kl* S 4 + (J.,+ J U ) S 2 ) *X refl 

- ( J Bl /Kl* s 2 + 1 ) * d is2 

t ref _ff2= ( Jo2* J L2 /K2* s 4 + (J n2 +J L2 ) s 2 ) *x ref2 

- ( J a2 /K2* s 2 + 1 ) *d ul ; and 

the non-interference position signal in the 
non-interference position signal producing unit is calculated 
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based upon the below-mentioned formulae: 

X re f_ffl= ( JLl/Kl* S 2 + l) * X ref x - 1 / K 1 * d is2 

x re f_ff 2 = ( Jl2/K2* s 2 + 1 ) *x ref2 -l/K2*d isl 

in the above-explained formulae, the respective symbols 
are defined as follows: 

J mi : first shaft motor inertia moment 

J L i : first shaft arm inertia moment 

Kl : spring constant of first shaft speed reducer 

J m 2 : second shaft motor inertia moment 

J L 2 : second shaft arm inertia moment 

K2 : spring constant of second shaft speed reducer 

A(=B) : coefficients calculated based upon, structures 

of two shafts, angle between two shafts, and geometrical 

relationship between two shafts 

s : Laplace operator 

disi : interference force exerted from first shaft to second 
shaft 

d iS 2 * interference force exerted from second shaft to 
first shaft 

Xrefi : first shaft position instruction 
x re f2 : second shaft position instruction. 
In the second aspect, the formulae for calculating the 
interference force, the non-interference torque signal, and 
the non-interference position signal can be specified. 
[0016] 

Further, according to a third aspect of the present 
invention, there is provided the robot control apparatus as 
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in the first aspect, wherein in the case that the shafts exceed 
two shafts, the calculation value of the interference force 
exerted from another shaft, which is used in the process 
operations by the non-interference torque signal producing unit 
and the non-interference position signal producing unit, is 
equal to a summation of interference force calculation values 
which are exerted from the respective shafts to the own shaft. 

In the third aspect, in such a case that interference 
force is exerted among a plurality of shafts, the interference 
force is received from the plural shafts even with respect to 
the own shaft. In this case, since a summation of the 
interference force between the two shafts is calculated, the 
interference force exerted to the own shaft can be calculated. 

Advantage of the Invention 
[0017] 

In accordance with the first aspect of the present 
invention, the motor can be operated in such a manner that the 
own shaft is operated in correspondence with the instruction 
also in such a case that the interference force exerted from 
another shaft is present. As a result, the robot arm can be 
operated in correspondence with the instruction without 
receiving the adverse influence by the interference. Further, 
the calculation capable of realizing the interference free 
operation can be considerably reduced. Accordingly, the 
calculation amount of the CPU can be largely reduced, so that 
the low cost CPU can be employed. 
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Further, in accordance with the second aspect, since the 
formulae for calculating the interference force, the 
non-interference torque signal, and the non-interference 
position signal are specified, the robot control operation can 
be embodied. 

Further, in accordance with the third aspect, also in 
such a case that interference force is exerted from the plural 
shafts to the own shaft, the process operation for calculating 
a total value of the interference force exerted from the 
respective shafts to the own shaft is merely carried out, while 
a relationship among the plural shafts need not be considered 
but also a complex calculation is not carried out . Subsequently, 
the interference free operations among the plural shafts can 
be simply carried out by merely performing a process operation 
similar to the inference free operation between the two shafts. 

Further, in the case that interference between shafts 
which have not been considered is added, while a program is 
not substantially amended, the interference free operations 
for all of the shafts can be simply realized by merely adding 
such an operation for calculating a total value of the 
interference force exerted from the respective shafts. 

Brief Description of the Drawings 

[0018] 

[Fig. 1] 

Fig. 1 is a block diagram for indicating an arrangement 
of a first embodiment of the present invention. 
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[Fig. 2] 

Fig. 2 is a block diagram for showing an arrangement of 
a second embodiment of the present invention. 
[Fig. 3] 

Fig. 3 represents a dynamic model used in the case that 
coefficients "A" and "B" required to calculate interference 
force are calculated. 
[Fig. 4] 

Fig. 4 indicates a control subject model in which while 
a mono shaft is defined as two inertia systems, mutual 
interference force is considered. 
[Fig.. 5] 

Fig. 5 is a block diagram for showing the arrangement 
of the conventional robot control apparatus. 
[Fig. 6] 

Fig. 6 is a block diagram for showing the process operations 
executed in the model controller of the conventional robot 
control apparatus . 

Description of Reference Numerals and Sings 
[0019] 

1 position control unit 

2 speed control unit 

3 differentiator 

4 differentiator 

5 non-interference positional signal producing unit 

6 non-interference torque signal producing unit 
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7 


interference force calculating unit 


8 


motor 


9 


arm 


10 


position detector 


11 


position control unit 


12 


speed control unit 


13 


differentiator 


14 


differentiator 


15 


non-interference positional signal producing unit 


16 


non-interference torque signal producing unit 


17 


interference force calculating unit 


18 


motor 


19 


arm 


20 


position detector 


38 


motor 


39 


arm 


30 


position detector 


51 


model controller 


52L, 


52U status monitoring device 


53L, 


53U feedback control system (feedback 


controller) 


54L, 


54U correction amount calculating unit 


60L, 


60U output signal which is outputted to feedback 


control system 


100 


control apparatus of first shaft robot 



200 control apparatus of second shaft robot 
300 control apparatus of n-th shaft robot 
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500 interference force adder 

Best Mode for Carrying Out the Invention 
[0020] 

Referring now to drawings, a description is made of 
embodiment modes of the present invention. 
EMBODIMENT 1 

[0021] 

Fig. 1 is an entire structural diagram for explaining 
a robot control apparatus of the present invention. In this 
drawing, reference numeral 100 shows a control apparatus of 
a first shaft robot, and reference numeral 200 indicates a 
control apparatus of a second shaft robot. 

The control apparatus 100 of the first shaft robot inputs 
a first shaft position instruction >% x re fi", a first shaft motor 
position detection value x> x fb i", and an interference force 
calculation value "d is2 " which is exerted from the first shaft 
to the second shaft, and performs a control calculation in such 
a manner that a control subject is operated in accordance with 
an instruction, and then, outputs a first shaft torque 
instruction "t ref i . " Reference numeral 8 indicates a first shaft 
motor, and is operated in response to the first shaft torque 
instruction "t re fi" outputted from the control apparatus 100 
of the fist shaft robot. Reference numeral 9 shows a first 
shaft arm, and the first shaft arm 9 has been coupled via a 
speed reducer and the like to the motor 8. Reference numeral 
10 represents a first shaft mot or posit ion detector which detects 
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a position "x fb i" of the first shaft motor 8. 

The control apparatus 200 of the second shaft robot inputs 
a second shaft position instruction "x ref2 " , a second shaft motor 
position detection value xx x fb2 ", and an interference force 
calculation value "d is i" which is exerted from the first shaft 
to the second shaft, and performs a control calculation in such 
a manner that a control subject is operated in accordance with 
an instruction, and then, outputs a second shaft torque 
instruction "t re f 2 ." Reference numeral 18 indicates a second 
shaft motor, and is operated in response to the second shaft 
torque instruction xx t re f 2 " outputted from the control apparatus 
200 of the second shaft robot. Reference numeral 19 shows a 
second shaft arm, and the second shaft arm 19 has been coupled 
via a speed reducer and the like to the motor 18. Reference 
numeral 20 represents a second shaft motor position detector 
which detects a position "x fh2 " of the second shaft motor 18. 

Both the first shaft and the second shaft have been omitted 
due to the following reasons. That is, normally, an outputted 
torque instruction value "t re f" is converted into a current 
instruction value, and the current instruction value is 
amplified by a current amplifier so as to operate a motor. A 
response of this operation is negligibly sufficiently fast, 
and has no specific relationship with the present invention. 
[0022] 

In this case, since the sped reducers which couple the 
motors 8 and 18 to the arm 9 and 19 own spring elements, any 
of control subjects of the robot control apparatus 100 and the 



24 



robot control apparatus 200 may be conceived as two inertia 
systems in which motor inertia moment is coupled to arm inertia 
moment by the spring element. 

In this embodiment, assuming now that a control subject 
for every single shaft is 2 inertia systems, such a structure 
is considered which is expressed by a transfer function shown 
in Fig. 4 and by considering interference occurred between the 
mutual shafts. 

In Fig. 4, respective symbols own the below-mentioned 
meanings : 

J m i : first shaft motor inertia moment 

Jli : first shaft arm inertia moment 

Kl : spring constant of first shaft speed reducer 

J m2 : second shaft motor inertia moment 

J L 2 : second shaft arm inertia moment 

K2 : spring constant of second shaft speed reducer 

A, B : coefficients calculated based upon, structures 

of two shafts, angle between two shafts, and geometrical 

relationship between two shafts 

s : Laplace operator 

disi : inter ference force exerted from first shaft to second 
shaft 

d is2 : interference force exerted from second shaft to 
first shaft 

xl : position of arm 1 
x2 : position of arm 2 

Further, the above-explained coefficients "A" and "B" 
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may be calculated based upon a dynamic calculation. 

For instance, such a case that two shafts own a relationship 
as indicated in Fig. 3 is considered. Assuming now that an 
angle defined between a first shaft and a second shaft is "9" ; 
a distance between the first shaft and the second shaft is 
expressed as "L"; distances measured from a rotational center 
to a center of mass as to each of the first and second shafts 
are expressed as "LI" and "L2"; and respective mass is expressed 
as Ml and M2, the coefficients "A" and "B" are given by the 
below-mentioned formula (9) : 

A=B=M2*L2*L*cos (9) +M2*L2 2 (9) 

[0023] 

Next, a description is actually made of an internal 
arrangement of the first shaft robot control apparatus 100. 

In Fig. 1, reference numeral 1 indicates a position control 
unit of the first shaft. The first shaft position control unit 
1 inputs first shaft position deviation "errl", and performs 
a control calculation so as to output a first shaft speed 
instruction "v re fi." The first shaft position deviation "errl" 
corresponds to a difference between a first shaft 
non-interference position signal "x re f_ffi" and a first shaft 
position detection value x> x fbl ", and the first shaft position 
control unit 1 inputs the first shaft position deviation "errl" 
in such a manner that the first shaft non-interference position 
signal "x re f_fn" is made coincident with the first shaft position 
detection value x> Xf b i." 

Although process operations of the position control unit 
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1 may be carried out in any manners, the position control unit 

1 may execute a proportional control operation by employing 
a first shaft position loop proportional gain "K p i" as indicated 
in, for example, formula (10): 

v r efi = Kpi * errl (10) 

Reference numeral 2 indicates a first shaft speed control 
unit. The first shaft speed control unit 2 inputs first shaft 
speed deviation "v er ri" which is calculated by the following 
formula (11) in such amanner that a first shaft non-interference 
speed signal "v re f_ffi" is made coincident with a first shaft 
speed detection value "v f bi", and performs a control calculation 
operation so as to output a first shaft torque ins truction value 

£ r ef_fbl • 

Verrl = V re f_ffi + V re fl " V fbl (11) 

[0024] 

Although process operations in the speed control unit 

2 may be carried out in any manners, the speed control unit 
2 may execute a proportional integration control operation by 
employing a first shaft speed loop gain proportional gain "K v i" 
and a first shaft speed loop integration gain "Kn" as represented 
in, for instance, the below-mentioned formula (12): 

t r ef_fbl = K v i (12) 

Reference numerals 3 and 4 show differentiators. 

Reference numeral 5 indicates a first shaft 
non-interference position signal producing unit. The first 
shaft non-interference position signal producing unit 5 inputs 
both the first shaft position instruction "x re fi" and the 



27 



interference force calculation value "d is2 " which is exerted 
from the second shaft to the first shaft, and produces a first 
shaft motor position signal by employing a relationship of the 
interference model of the two shafts shown in Fig. 4 so as to 
output this produced first shaft motor position signal as a 
first shaft non-interference position signal "x re f_ f fi . " The 
above-explained first shaft motor position signal is used in 
order that even when interference force >N d is2 " is exerted, a 
tip portion "xl" of the arm 9 is operated in correspondence 
with the first shaft position instruction "x re fi . " A calculation 
formula as to the first shaft non-interference position signal 
"x re f_fn" is expressed by the following formula (13) : 
X re f_ffl = ( J L i (13) 

[0025] 

Reference numeral 6 indicates a first shaft 
non-interference torque signal producing unit . The first shaft 
non-interference torque signal producing unit 6 inputs both 
the first shaft position instruction "x re fi" and the interference 
force calculation value "d is2 " which is exerted from the second 
shaft to the first shaft, and produces a first shaft motor torque 
instruction signal by employing a relationship of the 
interference model of the two shafts shown in Fig. 4 so as to 
output this produced first shaft mo tor torque instruction signal 
as a first shaft non-interference torque signal "t re f_ffi." The 
above-explained first shaft motor torque instruction signal 
is used in order that even when interference force "d is2 " is 
exerted, the tip portion "xl" of the arm 9 is operated in 
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correspondence with the first shaft position instruction xx x re fi . " 
A calculation formula as to the first shaft non-interference 
torque signal M t re f_fii" is expressed by the following formula 
(14) : 

tref_ffl = (Jli —(14) 

Reference numeral 7 shows a first shaft interference force 
calculating unit. The first shaft interference force 
calculating unit 7 inputs the first shaft position instruction 
xx x re fi" so as to calculate interference force x M is i" which is 
exerted from the first shaft to the second shaft, and then, 
outputs the calculated interference force "d is i." As apparent 
from Fig. 4, the interference force "d is i" is calculated based 
upon the below-mentioned formula (15): 

dsi = B*x ref i*s 2 — (15) 

Finally, a first shaft torque instruction value xx t re fi" 
which is outputted from the first shaft robot control apparatus 
100 to the motor 8 is calculated by the below-mentioned formula 
(16) : 

trefl ~ t re f_ffl + t re f_fbl (16) 

[0026] 

Next, an internal arrangement of the second shaft robot 
control apparatus 200 will now be described. 

In the drawing, reference numeral 11 indicates a position 
control unit of the second shaft. The second shaft position 
control unit 11 inputs second shaft position deviation "err2", 
and performs a control calculation so as to output a second 
shaft speed instruction w v re f 2 ." The second shaft position 
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deviation "err2" corresponds to a difference between a second 
shaft non-interference position signal xx x re f_ff2" and a second 
shaft position detection value "x fb2 ", and the second shaft 
position control unit 11 inputs the second shaft position 
deviation "err2" in such a manner that the second shaft 
non-interference position signal "x re f_ f f 2 " is made coincident 
with the second shaft position detection value x> x fb2 ." 

Although process operations of the position control unit 
11 may be carried out in any manners, the position control unit 

11 may execute a proportional control operation by employing 
a second shaft position loop proportional gain xx K p2 " as indicated 
in, for example, formula (17): 

v ref2 = K p2 * err2 ---(17) 

Reference numeral 12 indicates a second shaft speed 
control unit. The second shaft speed control unit 12 inputs 
second shaft speed deviation xx v er r2" which is calculated by the 
following formula (18) in such a manner that a second shaft 
non-interference speed signal xx v re f_ff 2 " is made coincident with 
a second shaft speed detection value xx Vf b 2"/ and performs a 
control calculation operation so as to output a second shaft 
torque instruction value xx t re f_fb2" : 

V e rr2 = V ref __ff 2 + V ref2 V fb2 (18) 

[0027] 

Although process operations in the speed control unit 

12 may be carried out in any manners, the speed control unit 
12 may execute a proportional integration control operation 
by employing a second shaft speed loop gain proportional gain 



"Ki2" and a second shaft speed loop integration gain "Kij" as 
represented in, for instance, the below-mentioned formula (19) : 

tref_fb2 = K v2 * d + K i2 /s) * V G rr2 (19) 

Reference numerals 13 and 14 show differentiators. 

Reference numeral 15 shows a second shaft 
non-interference position signal producing unit. The second 
shaft non-interference position signal producing unit 15 inputs 
both the interference force calculation value "d is i" which is 
exerted from the first shaft to the second shaft, and the second 
shaft position instruction "x re f2"/ and forms a second shaft 
motor position signal by employing a relationship of the 
inference model of the two shafts shown in Fig. 4 so as to output 
this produced second shaft motor position signal as a second 
shaft non-interference position signal "x re f_ff2 . " The second 
shaft motor position signal is used in order that even when 
the interference force "d is i" is exerted, the tip portion "x2" 
of the arm 19 is operated in correspondence with the second 
shaft position instruction "x ref2 . " A calculation formula as 
to the second shaft non-interference position signal M x re f_ff 2 // 
is represented in the below-mentioned formula (20) : 

X re f_ff2 = (J L2 /K2*S 2 +1) *X re f2" 1/K2*d is l "—(20) 

[0028] 

Reference numeral 16 indicates a second shaft 
non-interference torque signal producing unit. The second 
shaft non-interference torque signal producing unit 16 inputs 
both the second shaft position instruction M x ref2 " and the 
interference force calculation value "d is i" which is exerted 
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from the first shaft to the second shaft, and produces a second 
shaft motor torque instruction signal by employing a 
relationship of the interference model of the two shafts shown 
in Fig. 4 so as to output this produced second shaft motor torque 
instruction signal as a second shaft non-interference torque 
signal "t re f_ff2 • " The above-explained second shaft motor torque 
instruction signal is used in order that even when interference 
force "disi" is exerted, the tip portion "x2" of the arm 19 is 
operated in correspondence with the second shaft position 
instruction "x re f2 - " A calculation formula as to the second shaft 
non-interference torque signal "t re f_ff2" is expressed by the 
following formula (21): 

t r ef_ff2 = (Jm2*JL2/K2*S 4 + (21) 

Reference numeral 17 shows a second shaft interference 
force calculating unit. The second shaft interference force 
calculating unit 17 inputs the second shaft position instruction 
"x re f2" so as to calculate interference force "d is2 " which is 
exerted from the second shaft to the first shaft, and then, 
outputs the calculated interference force "di S 2." As apparent 
from Fig. 4, the interference force M d iS 2" is calculated based 
upon the below-mentioned formula (22) : 

d is2 = A*x ref2 *s 2 (22) 

Finally, the second shaft torque instruction value "t re f2" 
which is outputted from the second shaft robot control apparatus 
2 00 to the motor 18 is calculated by the below-mentioned formula 
(23) : 

t r ef2 = t re f ff2 + t r ef fb2 (23) 
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[0029] 

As previously explained, the robot control apparatus is 
arranged so as to perform the feedback control operation by 
producing the motor position signal and the motor torque 
instruction signal in such a manner that even when the 
interference force is exerted, the respective shaft arms 9 and 
19 may be operated in correspondence with the instructions for 
the respective shafts, and then, the motors 8 and 18 may be 
operated in accordance with these instructions. As a result, 
even in such a case that shaft-to-shaft interference occurs, 
the respective shaft arms 9 and 19 can be operated in response 
to the instructions of the own shafts. 

The present invention owns the following different 
portions from those of the patent publication 1. That is, the 
robot control apparatus of the present invention is equipped 
with the non-interference position signal producing unit and 
the non-interference torque signal producing unit, while this 
robot control apparatus does not contain the pseudomodel and 
the model-purpose controller. The different portions are 
featured to produce not only the motor torque correction signal, 
but also the motor position signal by considering the 
interference . 

EMBODIMENT 2 

[0030] 

Fig. 2 is a diagram for indicating an arrangement of a 
second embodiment . A different point of this second embodiment 
from that of the first embodiment is given as follows: That 
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is, in the first embodiment, the interference of the two shafts 
is considered, whereas in this second embodiment, such a case 
that interference force is exerted among a plurality of shafts 
is considered. 

As indicated in Fig. 2, a robot which is constituted by 
w n" pieces of shafts will now be considered. In this case, 
an interference free operation of a first shaft in the case 
that interference is exerted to the first shaft from a second 
shaft through an "n"-th shaft respectively is exemplified. A 
completely similar interference free operation may be realized 
as to also interference among other shafts. 

In this drawing, reference numeral 38 indicates an n-th 
shaft motor which is operated in response to an n-th shaft torque 
instruction "t re f n " which is outputted from a control apparatus 
300 of an "n"-th shaft robot. Reference numeral 39 indicates 
an "n"-th shaft arm which has been coupled via a speed reducer 
and the like to a motor 38. Reference numeral 30 indicates 
a "n"-th shaft motor position detector which detects a position 
"Xfbn" of the n-th shaft motor 38. 

Reference numeral 500 shows an interference force adder. 
The interference force adder 500 executes such a process 
operation as represented in the below-mentioned formula (24) , 
while this interference force adder 500 calculates a summation 
"d is _aii" by adding plural interference force calculation values 
from an interference force calculation value "d iS 2" which is 
exerted from the second shaft to the first shaft up to an 
interference force calculation value "d iS n" which is exerted 
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from the "n"-th shaft to the first shaft: 

d is _all = d is2 + d is 3 + + d isn (24) 

In this formula, similar to the calculation explained 
by employing Fig. 3 in the embodiment 1, the interference force 
calculation values from "d is2 " to "d iS n" are obtained based upon 
the structures of the respective shafts from the first shaft 
up to the n-th shaft, angles among the shafts, and a geometrical 
relationship among the shafts. 
[0031] 

As previously explained, even in such a case that the 
interference is exerted among the plural shafts, while the 
summation value of the interference force calculation values 
is inputted which are exerted from the respective shafts to 
the own shaft, a similar process operation to that of the first 
embodiment is merely carried out . As a result, the interference 
free operations among all of the shafts can be carried out by 
a simple arrangement and by reducing a calculation amount. 

Further, in the case that shafts are increased, and such 
an interference which has not been considered is newly considered, 
while all of the process operations for the respective shafts 
arenot completely changed, aportion for calculating a summation 
of interference force calculation values among the respective 
shafts may be merely added and merely corrected. As a result, 
the above-explained cases can be simply accepted. 

Industrial Applicability 
[0032] 
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The present invention can be utilized in control apparatus 
of robots with employment of a low-cost CPU, and also in 
interference free control operations for robots having a large 
number of shafts, since the calculation amount is small and 
the interference free operations of the robots can be realized 
even when the interference force is exerted among the plural 
shafts . 
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